{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "60d186d5",
   "metadata": {},
   "source": [
    "# 单轴调试"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8708012",
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "# 添加pcan_cybergear库的路径\n",
    "sys.path.append(os.path.join(\"..\", \"cybergear\"))\n",
    "\n",
    "from pcan_cybergear import CANMotorController\n",
    "import can\n",
    "import logging\n",
    "# Initialize logging\n",
    "logging.basicConfig(level=logging.WARNING, format='%(asctime)s - %(levelname)s - %(message)s')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcee4222",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Connect to the CAN bus with 1 Mbit/s bitrate\n",
    "bus = can.interface.Bus(bustype=\"pcan\", channel=\"PCAN_USBBUS1\", bitrate=1000000)\n",
    "motor1 = CANMotorController(bus, motor_id=101, main_can_id=254)\n",
    "motor2 = CANMotorController(bus, motor_id=102, main_can_id=254)\n",
    "motor3 = CANMotorController(bus, motor_id=103, main_can_id=254)\n",
    "motor4 = CANMotorController(bus, motor_id=104, main_can_id=254)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b91bd39f",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor = motor2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f3a086fa",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"limit_cur\", 1.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "40adfc24",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"cur_filt_gain\", 0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e51c66a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"loc_kp\", 8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "31377c1a",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"spd_kp\", 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7c01cce",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"spd_ki\", 0.03)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2f4fb319",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"cur_kp\", 0.0125) # 默认0.125"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78152227",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"cur_ki\", 0.1) # 默认0.0158"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "65d753a4",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_param_table(\"spd_filt_gain\", 0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "86cb5366",
   "metadata": {},
   "source": [
    "## 停止"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d0ea5625",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fdcef3d0",
   "metadata": {},
   "source": [
    "# 位置模式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c280a67",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_run_mode(motor.RunModes.POSITION_MODE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bca0d55c",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.enable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a07fba09",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "motor.write_single_param(\"loc_ref\", value=-0.5)\n",
    "time.sleep(1.5)\n",
    "motor.write_single_param(\"loc_ref\", value=0.5)\n",
    "time.sleep(1.5)\n",
    "motor.write_single_param(\"loc_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc433bc1",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"limit_spd\", value=4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "54bbaea6",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"loc_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd6fd4da",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_motor_position_control(1, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98831b4d",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9eee2db3",
   "metadata": {},
   "source": [
    "# 运控模式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b403f0b",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_run_mode(motor.RunModes.CONTROL_MODE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c19da7ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.enable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72dd5fb2",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.send_motor_control_command(torque=0, target_angle=0, target_velocity=0, Kp=1, Kd=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "200fdff2",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5bc013bd",
   "metadata": {},
   "source": [
    "# 速度模式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e61aef81",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_run_mode(motor.RunModes.SPEED_MODE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "489bb6b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.enable()\n",
    "motor.write_single_param(\"spd_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cab3fa45",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1fb975b6",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"spd_ref\", value=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d9b4a798",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "start_time = time.time()\n",
    "test_time = 15\n",
    "speed = 2\n",
    "motor.enable()\n",
    "while True:\n",
    "    elapsed_time = time.time() - start_time\n",
    "    if elapsed_time >= 10:\n",
    "        logging.warning(f\"{test_time} seconds have passed. End test\")\n",
    "        motor.set_0_pos()\n",
    "        motor.disable()\n",
    "        break\n",
    "    _, pos, vel, torque_ext = motor.write_single_param(\"spd_ref\", value=speed)\n",
    "    if abs(torque_ext) > 0.2:\n",
    "        speed = -speed\n",
    "        motor.write_single_param(\"spd_ref\", value=speed)\n",
    "        logging.warning(f\"reverse on torque_ext: {torque_ext:.2f}, vel:{vel:.2f}\")\n",
    "    time.sleep(0.01)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "af3b0683",
   "metadata": {},
   "source": [
    "# 电流模式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15b3a087",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_run_mode(motor.RunModes.CURRENT_MODE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2e8208f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# iq_ref\n",
    "motor.enable()\n",
    "motor.write_single_param(\"iq_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7059a013",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"iq_ref\", value=0.2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2a16cfdc",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "458eb159",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
